/*

Matrix transformations.

Greg Turk, November 1996.

*/

#include <stdio.h>
#include <math.h>
#include "matlib.h"

extern float mat_invert (gtMatrix);





/******************************************************************************
Print out a matrix.
******************************************************************************/

void print_matrix(gtMatrix mat)
{
  int i,j;

  for (j = 0; j < 4; j++) {
    for (i = 0; i < 4; i++)
      printf ("%f ", mat[i][j]);
    printf ("\n");
  }
}


/******************************************************************************
Copy a matrix.

Entry:
  dest   - destination matrix
  source - source matrix to copy
******************************************************************************/

void copy_matrix(gtMatrix dest, gtMatrix source)
{
  int i,j;

  for (i = 0; i < 4; i++)
    for (j = 0; j < 4; j++)
      dest[i][j] = source[i][j];
}


/******************************************************************************
Set a matrix to the identity matrix.
******************************************************************************/

void identity_matrix(gtMatrix mat)
{
  int i,j;

  for (i = 0; i < 4; i++)
    for (j = 0; j < 4; j++)
      mat[i][j] = (i == j);
}


/******************************************************************************
Transpose a matrix.
******************************************************************************/

static void transpose_matrix(gtMatrix m)
{
  int i,j;
  gtMatrix m2;

  /* copy */
  copy_matrix (m2, m);

  /* transpose */
  for (i = 0; i < 4; i++)
    for (j = 0; j < 4; j++)
      m[i][j] = m2[j][i];
}


/******************************************************************************
Multiply two matrices.

Entry:
  m1,m2 - matrices to multipy

Exit:
  dest - result of m1 * m2
******************************************************************************/

void mult_matrix(gtMatrix dest, gtMatrix m1, gtMatrix m2)
{
  int i,j,k;
  gtMatrix m;

  /* perform multiplication */

  for (i = 0; i < 4; i++)
    for (j = 0; j < 4; j++) {
      m[i][j] = 0;
      for (k = 0; k < 4; k++)
	m[i][j] += m1[k][j] * m2[i][k];
    }

  /* copy the answer */

  for (i = 0; i < 4; i++)
    for (j = 0; j < 4; j++)
      dest[i][j] = m[i][j];
}


/******************************************************************************
Apply a matrix transformation to a vector.

Entry:
  v - vector to transform
  m - matrix to apply

Exit:
  v - transformed vector
******************************************************************************/

void apply_matrix(gtVector v, gtMatrix m)
{
  int i;
  gtVector vtemp;

  /* multiply the matrix times the vector */
 
  for (i = 0; i < 3; i++)
    vtemp[i] = v[0] * m[0][i] + v[1] * m[1][i] + v[2] * m[2][i] + m[3][i];

  /* copy the result into the destination */

  for (i = 0; i < 3; i++)
    v[i] = vtemp[i];
}


/******************************************************************************
Normalize a vector.
******************************************************************************/

void normalize_vector(gtVector v)
{
  float len,recip;

  len = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];

  if (len == 0) {
    fprintf (stderr, "normalize_vector: zero length vector\n");
    return;
  }

  recip = 1.0 / sqrt (len);
  v[0] *= recip;
  v[1] *= recip;
  v[2] *= recip;
}


/******************************************************************************
Print length of vector.  For debugging.
******************************************************************************/

void print_length(gtVector v)
{
  float len,recip;

  len = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];

  if (len == 0) {
    fprintf (stderr, "zero length vector\n");
    return;
  }

  len = sqrt(len);
  printf ("len = %f\n", len);
}


/******************************************************************************
Form the cross-product of two vectors.

Entry:
  a,b - vectors to form cross-product of

Exit:
  c - a cross b
******************************************************************************/

void cross(gtVector c, gtVector a, gtVector b)
{
  gtVector vtemp;

  /* perform cross-product */
  vtemp[0] = a[1]*b[2] - a[2]*b[1];
  vtemp[1] = a[2]*b[0] - a[0]*b[2];
  vtemp[2] = a[0]*b[1] - a[1]*b[0];

  /* copy the result */
  c[0] = vtemp[0];
  c[1] = vtemp[1];
  c[2] = vtemp[2];
}


/******************************************************************************
Subtract one vector from another.

Entry:
  a,b - vectors to subtract

Exit:
  c = a minus b
******************************************************************************/

void vector_subtract(gtVector c, gtVector a, gtVector b)
{
  c[0] = a[0] - b[0];
  c[1] = a[1] - b[1];
  c[2] = a[2] - b[2];
}


/******************************************************************************
Add one vector to another.

Entry:
  a,b - vectors to add

Exit:
  c = a plus b
******************************************************************************/

void vector_add(gtVector c, gtVector a, gtVector b)
{
  c[0] = a[0] + b[0];
  c[1] = a[1] + b[1];
  c[2] = a[2] + b[2];
}


/******************************************************************************
Copy one vector to another.

Entry:
  source - vector to copy from

Exit:
  dest - vector to copy to
******************************************************************************/

void vector_copy(gtVector dest, gtVector source)
{
  dest[0] = source[0];
  dest[1] = source[1];
  dest[2] = source[2];
}


/******************************************************************************
Initialize the matrix stack.
******************************************************************************/

void gtSimulator::gtInitialize()
{
  identity_matrix (stack[0].mat);
}


/******************************************************************************
Push the matrix stack.
******************************************************************************/

void gtSimulator::gtPushMatrix()
{
  /* check for stack overflow */

  if (stack_top + 1 == STACK_MAX) {
    fprintf (stderr, "Too many matrix pushes\n");
    return;
  }

  /* replicate the top matrix */
  copy_matrix (stack[stack_top+1].mat, stack[stack_top].mat);
  stack_top++;
}


/******************************************************************************
Pop the matrix stack.
******************************************************************************/

void gtSimulator::gtPopMatrix()
{
  /* check for stack underflow */

  if (stack_top == 0) {
    fprintf (stderr, "Matrix stack underflow\n");
    return;
  }

  /* forget the top of the stack */

  stack_top--;
}


/******************************************************************************
Return the top matrix of the matrix stack.
******************************************************************************/

void gtSimulator::get_top_matrix(gtMatrix mat)
{
  copy_matrix (mat, stack[stack_top].mat);
}


/******************************************************************************
Append a translation matrix to the current transformation matrix.

Entry:
  tx,ty,tz - translation amounts in x,y,z
******************************************************************************/

void gtSimulator::gtTranslate(float tx, float ty, float tz)
{
  gtMatrix mat;

  identity_matrix (mat);
  mat[3][0] = tx;
  mat[3][1] = ty;
  mat[3][2] = tz;

  mult_matrix (stack[stack_top].mat, stack[stack_top].mat, mat);
}


/******************************************************************************
Append a scaling matrix to the current transformation matrix.

Entry:
  sx,sy,sz - scaling amounts in x,y,z
******************************************************************************/

void gtSimulator::gtScale(float sx, float sy, float sz)
{
  gtMatrix mat;

  identity_matrix (mat);
  mat[0][0] = sx;
  mat[1][1] = sy;
  mat[2][2] = sz;

  mult_matrix (stack[stack_top].mat, stack[stack_top].mat, mat);
}


/******************************************************************************
Append a rotation matrix to the current transformation matrix.

Entry:
  angle    - number of degrees to rotate around a given axis
  ax,ay,az - the given axis
******************************************************************************/

void gtSimulator::gtRotate(float angle, float ax, float ay, float az)
{
  gtMatrix r;
  gtMatrix r1,r2,r3;
  float theta;
  gtVector n;
  gtVector a,b,c;

  a[0] = ax;
  a[1] = ay;
  a[2] = az;
  normalize_vector(a);

  /* create vector not parallel to "a" */

  ax = fabs(a[0]);
  ay = fabs(a[1]);
  az = fabs(a[2]);

  n[0] = n[1] = n[2] = 0;

  if (ax > ay) {
    if (ay > az)
      n[2] = 1;    /* z is smallest */
    else
      n[1] = 1;    /* y is smallest */
  }
  else {
    if (ax > az)
      n[2] = 1;    /* z is smallest */
    else
      n[0] = 1;    /* x is smallest */
  }

  /* create "b" orthogonal to "a" */

  cross (b, a, n);
  normalize_vector(b);

  /* create "c" orthogonal to "a" and "b" */

  cross (c, a, b);

  /* make matrix that rotates a,b,c into x,y,z axes */

  identity_matrix (r1);
  r1[0][0] = a[0];
  r1[1][0] = a[1];
  r1[2][0] = a[2];
  r1[0][1] = b[0];
  r1[1][1] = b[1];
  r1[2][1] = b[2];
  r1[0][2] = c[0];
  r1[1][2] = c[1];
  r1[2][2] = c[2];

  /* make matrix for rotation by theta degrees around x-axis */

  theta = angle * 3.1415926535 / 180.0;
  identity_matrix (r2);
  r2[1][1] =  cos(theta);
  r2[2][2] =  cos(theta);
  r2[1][2] =  sin(theta);
  r2[2][1] = -sin(theta);

  /* make matrix that is inverse of r1 */
  copy_matrix (r3, r1);
  transpose_matrix (r3);

  /* compose these matrices for final matrix r = r3 * r2 * r1 */
  mult_matrix (r, r3, r2);
  mult_matrix (r, r, r1);

  /* append to current transformation matrix */
  mult_matrix (stack[stack_top].mat, stack[stack_top].mat, r);
}


/******************************************************************************
Specify a viewing matrix and multiply the current transformation by this.

Entry:
  fx,fy,fz - our view position
  ax,ay,az - point to look at
  ux,uy,uz - up vector (from the origin)
******************************************************************************/

void gtSimulator::gtLookAt(
  float fx,
  float fy,
  float fz,
  float ax,
  float ay,
  float az,
  float ux,
  float uy,
  float uz
)
{
  gtVector u,x,y,z;
  gtMatrix r,t,v;

  /* form a vector in the direction from "from" to "at" */
  z[0] = ax - fx;
  z[1] = ay - fy;
  z[2] = az - fz;
  normalize_vector(z);

  /* find vector "x" perpendicular to "z" and "up" */
  u[0] = ux;
  u[1] = uy;
  u[2] = uz;
  cross (x, z, u);
  normalize_vector(x);

  /* find vector "y" perpendicular to "x" and "z" */
  cross (y, x, z);

  /* create matrix that rotates "x" "y" and "z" into the x, y and z axes */

  identity_matrix (r);
  r[0][0] = x[0];
  r[1][0] = x[1];
  r[2][0] = x[2];
  r[0][1] = y[0];
  r[1][1] = y[1];
  r[2][1] = y[2];
  r[0][2] = -z[0];
  r[1][2] = -z[1];
  r[2][2] = -z[2];

  /* translate "from" to the origin */

  identity_matrix (t);
  t[3][0] = -fx;
  t[3][1] = -fy;
  t[3][2] = -fz;

  /* view matrix = rotation * translation */

  mult_matrix (v, r, t);

  /* apply to the current transformation matrix */

  mult_matrix (stack[stack_top].mat, stack[stack_top].mat, v);
}


/******************************************************************************
Apply the current transformation matrix to a given vector.
******************************************************************************/

void gtSimulator::apply_current_transformation (gtVector v)
{
  apply_matrix (v, stack[stack_top].mat);
}


/******************************************************************************
Apply the inverse-transpose of the current transformation matrix to a given
vector.
******************************************************************************/
